#include "imagematcher.h"
#include <map>
#include "stitcher.h"
#include <boost/foreach.hpp>
#include "imagefeature.h"
#include "inputimage.h"
#include "imagematch.h"
#include "imageset.h"

using namespace boost;
using namespace std;
using namespace cshotel;
using namespace sift;

typedef pair<weak_ptr<InputImage>, int> NeighbourPair1;
typedef pair<int, weak_ptr<InputImage> > NeighbourPair2;

ImageMatcher::ImageMatcher(shared_ptr<ImageSet> imgs) : imageSet(imgs) { }

void ImageMatcher::match(int numNeighbours) {
    dout("Matching images ..." << endl);
    
    // iterate over all images
    foreach(shared_ptr<InputImage> image, imageSet->getImages()) {
        dout("    matching image " << image->getId() << " ..." << endl);
        map<weak_ptr<InputImage>, int> neighbours;
        
        // look at each feature to get the number of matching features
        // with every other image
        foreach(shared_ptr<ImageFeature> feature, image->getImageFeatures()) {
            foreach(weak_ptr<ImageFeature> match, feature->getFeatureMatches()) {
                if(shared_ptr<ImageFeature> matchingFeature = (match.lock())) {
                    if(InputImage::getImageId(matchingFeature->getImage()) != image->getId()) {
                        ++neighbours[matchingFeature->getImage()];
                    }
                }
            }
        }
        
        // sort the images by matching features
        multimap<int, weak_ptr<InputImage> > sortedNeighbours;
        foreach(NeighbourPair1 neighbour, neighbours) {
            dout("        found " << neighbour.second << " matches with image " << InputImage::getImageId(neighbour.first) << endl);
            sortedNeighbours.insert(NeighbourPair2(neighbour.second, neighbour.first));
        }
        
        // try to match the images with most matching features
        int numInserted = 0;
        reverse_foreach(NeighbourPair2 neighbour, sortedNeighbours) {
            shared_ptr<ImageMatch> p(new ImageMatch(image, neighbour.second));
            
            // calculate the homography
            p->doRANSAC();
            
            // only accept good matches
            if(p->doProbabilisticCheck()) {
                image->addImageMatch(p);
                dout("        matched to image " << InputImage::getImageId(neighbour.second) << endl);
            } else {
                dout("        discarded match with image " << InputImage::getImageId(neighbour.second) << endl);
            }
            dout("            " << neighbour.first << " feature matches: " << p->getInliers().size() << " inliers " << p->getOutliers().size() << " outliers" << endl);
            ++numInserted;
            if(numInserted >= numNeighbours) {
                break;
            }
        }
    }
}
